
#include "member_BSliderRestraint.h"
#include "member_BRestraint.h"
#include "member_BNode.h"

using namespace BWE;

member_BSliderRestraint::member_BSliderRestraint(BSliderRestraint* sliderRestraint)
{
	boss = sliderRestraint;

	axisA.set(1, 0, 0);
	axisB.set(1, 0, 0);

	linearLimited = false;
	angularLimited = false;
	linearDiff = 0;
	linearPos = 0;
	angularDiff = 0;
	angularPos = 0;

	linearMotorEnabled = false;
	linearMotorTarget = 0;
	linearMotorVelocity = 0;
	linearMotorMaxForce = 0;
	linearMotorReached = false;

	angularMotorEnabled = false;
	angularMotorTarget = 0;
	angularMotorVelocity = 0;
	angularMotorMaxForce = 0;
	angularMotorReached = false;
}
member_BSliderRestraint::~member_BSliderRestraint()
{

}

void member_BSliderRestraint::linearCaculate()
{
	const BBody* bodyA = restraint_member(boss)->bodyA;
	const BBody* bodyB = restraint_member(boss)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	BVector pointA = matrixA * restraint_member(boss)->pivotA;
	BVector pointB = matrixB * restraint_member(boss)->pivotB;

	BVector offset = pointB - pointA;
	BVector sliderAxis = matrixA.mult3(axisA);
	linearPos += offset.dot(sliderAxis);

	linearDiff = 0;
	linearLimited = false;
	if (linearLimit.min() > linearLimit.max())
		return;
	if (linearPos > linearLimit.max())
	{
		linearDiff = linearPos - linearLimit.max();
		linearLimited = true;
	}
	else if (linearPos < linearLimit.min())
	{
		linearDiff = linearPos - linearLimit.min();
		linearLimited = true;
	}
}
BReal member_BSliderRestraint::linearMotorFactor(BReal stepTime)
{
	BReal value = linearPos;
	BReal target = linearMotorTarget;
	BReal lenght = 0;
	if (stripe && stripe->closeLoop())
		lenght = stripe->length();
	else if (curve && curve->closeLoop())
		lenght = curve->length();
	if (lenght > 0)
	{
		if (target < 0)
			target += lenght;
		if (value < 0)
			value += lenght;
	}

	BReal factor = 1;
	BReal delta = abs(linearMotorVelocity) * stepTime;
	if (!linearMotorReached)
	{
		if (abs(target - value) < delta)
		{
			linearMotorReached = true;
			boss->emit(Signal_LinearMotorReached, true);
		}
	}
	if (linearMotorReached || lenght == 0)
	{
		if (linearMotorVelocity > REAL_EPSILON)
		{
			if (value < target)
			{
				if (target - value < delta)
					factor = (target - value) / delta;
				else
					factor = 1;
			}
			else
			{
				if (value - target < delta)
					factor = (target - value) / delta;
				else
					factor = -1;
			}
		}
		if (linearMotorVelocity < -REAL_EPSILON)
		{
			if (value > target)
			{
				if (value - target < delta)
					factor = (value - target) / delta;
				else
					factor = 1;
			}
			else
			{
				if (target - value < delta)
					factor = (value - target) / delta;
				else
					factor = -1;
			}
		}
	}
	return factor;
}

void member_BSliderRestraint::angularCaculate()
{
	const BBody* bodyA = restraint_member(boss)->bodyA;
	const BBody* bodyB = restraint_member(boss)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	BVector pa, qa;
	PlaneSpace(axisA, pa, qa);
	const BVector axisA1 = matrixA.mult3(pa);
	const BVector axisA2 = matrixA.mult3(qa);
	
	BVector pb, qb;
	PlaneSpace(axisB, pb, qb);
	
	const BVector axisB0 = matrixB.mult3(pb);
	
	angularPos = atan2(axisB0.dot(axisA2), axisB0.dot(axisA1));
	angularPos = LimitAngle(angularPos, angularLimit.min(), angularLimit.max());

	//BMatrix frameA = BQuater(BVector(1, 0, 0), axisA.normal());
	//BMatrix frameB = BQuater(BVector(1, 0, 0), axisB.normal());
	//dirAxis = matrixA.mult3(frameA.row(0));
	//horAxis = matrixA.mult3(frameA.row(1));
	//verAxis = matrixA.mult3(frameA.row(2));
	//swingAxis = matrixB.mult3(frameB.row(1));
	//BReal Y = swingAxis.dot(verAxis);
	//BReal X = swingAxis.dot(horAxis);
	//angularPos = atan2(Y, X);

	angularDiff = 0;
	angularLimited = false;
	if (angularLimit.min() > angularLimit.max())
		return;
	if (angularPos < angularLimit.min())
	{
		angularDiff = angularLimit.min() - angularPos;
		angularLimited = true;
	}
	else if (angularPos > angularLimit.max())
	{
		angularDiff = angularLimit.max() - angularPos;
		angularLimited = true;
	}
}
BReal member_BSliderRestraint::angularMotorFactor(BReal stepTime)
{
	BReal value = angularPos;
	BReal target = angularMotorTarget;
	if (abs(target) > -PI2 && abs(target) < PI2 && abs(target) > PI / 2)
	{
		if (target < 0)
			target += PI2;
		if (value < 0)
			value += PI2;
	}
	BReal factor = 1;
	BReal delta = abs(angularMotorVelocity) * stepTime;
	if (!angularMotorReached)
	{
		if (abs(target - value) < delta)
		{
			angularMotorReached = true;
			boss->emit(Signal_AngularMotorReached, true);
		}
	}
	if (angularMotorReached)
	{
		if (angularMotorVelocity > REAL_EPSILON)
		{
			if (value < target)
			{
				if (target - value < delta)
					factor = (target - value) / delta;
				else
					factor = 1;
			}
			else
			{
				if (value - target < delta)
					factor = (target - value) / delta;
				else
					factor = -1;
			}
		}
		if (angularMotorVelocity < -REAL_EPSILON)
		{
			if (value > target)
			{
				if (value - target < delta)
					factor = (value - target) / delta;
				else
					factor = 1;
			}
			else
			{
				if (target - value < delta)
					factor = (value - target) / delta;
				else
					factor = -1;
			}
		}
	}
	return factor;
}

void member_BSliderRestraint::slotLineStripeFlushed(BObject* object, const BValue& value)
{
	linearLimit.set(0, stripe->length());
}
void member_BSliderRestraint::slotSplineCurveFlushed(BObject* object, const BValue& value)
{
	linearLimit.set(0, curve->length());
}


